/*
 * drive.c
 *
 *  Created on: Jan 3, 2024
 *      Author: 80424
 */
#include "drive.h"

/*
 * FOC位置环控制模式 无零点检测
 * 不启用 或者说 没啥用
 * */
void FOC_PositionMode(float POSE,uint16_t Angle)
{
	/****位置模式下的PID计算任务********/
	/*控制频率1khz*/
	POSE_PIDStruct.target=POSE;//获取期望职位;
	POSE_PIDStruct.feedback=(float)Angle;//输入采样位置
	pid_pos_update(&POSE_PIDStruct);
	/*根据赋值修改矢量相位*/
	if(POSE_PIDStruct.out>0.0f)//判断力矩方向
	{
		rtU.q=POSE_PIDStruct.out;
		Moto_Direction=1;//换相位
	}
	else
	{
		rtU.q=-POSE_PIDStruct.out;
		Moto_Direction=0;
	}
}
/*
 * 就近位置的位置环
 * @parm:POSE 期望位置
 * @parm:Angle 当前编码器的采样位置
 * @note:矢量模被影响 ->rtU.q;
 * */
void FOC_Nearest_PositionMode(float POSE,uint16_t Angle)
{
	int16_t Opposite;
	/****位置模式下的PID计算任务********/
	/*控制频率1khz*/

	Opposite=POSE-Angle;//目标值减去当前值

	if(abs(Opposite)<2048)//需要旋转的位置小于半圈
	{
		POSE_PIDStruct.target=POSE;//获取期望职位;
		POSE_PIDStruct.feedback=(float)Angle;//输入采样位置

		pid_pos_update(&POSE_PIDStruct);
		/*根据赋值修改矢量相位*/
		if(POSE_PIDStruct.out>0.0f)//判断力矩方向
		{
			rtU.q=POSE_PIDStruct.out;
			Moto_Direction=1;//换相位
		}
		else
		{
			rtU.q=-POSE_PIDStruct.out;
			Moto_Direction=0;
		}
	}
	if(Opposite>=2048)//大于正向半圈
	{
		POSE_PIDStruct.target=POSE;//获取期望职位;
		POSE_PIDStruct.feedback=(float)Angle+4096;//输入采样位置

		pid_pos_update(&POSE_PIDStruct);
		/*根据赋值修改矢量相位*/
		if(POSE_PIDStruct.out>0.0f)//判断力矩方向
		{
			rtU.q=POSE_PIDStruct.out;
			Moto_Direction=1;//换相位
		}
		else
		{
			rtU.q=-POSE_PIDStruct.out;
			Moto_Direction=0;
		}

	}
	if(Opposite<=-2048)
	{
		POSE_PIDStruct.target=POSE+4096;//获取期望职位;
		POSE_PIDStruct.feedback=(float)Angle;//输入采样位置

		pid_pos_update(&POSE_PIDStruct);
		/*根据赋值修改矢量相位*/
		if(POSE_PIDStruct.out>0.0f)//判断力矩方向
		{
			rtU.q=POSE_PIDStruct.out;
			Moto_Direction=1;//换相位
		}
		else
		{
			rtU.q=-POSE_PIDStruct.out;
			Moto_Direction=0;
		}
	}
}
/*
 * 编码器微分得到速度
 * */
float Encoder_Speed(uint16_t encoder,float dt)
{

	static uint16_t last_encoder=0;

	int16_t encoder_error;


	if(encoder>3072&&last_encoder<1024)
	{
		encoder_error=encoder-(last_encoder+4096);
	}
	else if(encoder<1024 && last_encoder>3072)
	{
		encoder_error=(encoder+4096)-last_encoder;
	}
	else
	{
		encoder_error=encoder-last_encoder;
	}

	last_encoder=encoder;
	return encoder_error/dt;
}
/*
 * 速度闭环驱动
 * */
void FOC_SpeedCloseloop(float target_speed,float feedback_speed)
{
	SPEED_PIDSreuct.target=target_speed;
	SPEED_PIDSreuct.feedback=feedback_speed;
	pid_pos_update(&SPEED_PIDSreuct);
	rtU.q=SPEED_PIDSreuct.out;
}
